#include "AStar.h"

AStar::AStar(position depart,position fin,int  (&costmap)[NY][NX] ):costmap_(costmap),depart_(depart),fin_(fin)
{
    chemin_=false;
}


void AStar::a_star()
{
    open_.clear();
    closed_.clear();
    courant_.p=depart_;
    courant_.g=0;
    courant_.f=distance(depart_,fin_);
    courant_.parent=depart_;
    open_[courant_.p]=courant_;

    while(courant_.p!=fin_ && !(open_.empty())){
        meilleur_noeud();
        ajout_liste_ferme();
        ajout_case_adjacentes();

        }
    if(courant_.p!=fin_)
        chemin_=false;
    else
        chemin_=true;

}

void AStar::meilleur_noeud()
{
    map<position,noeud>::iterator it;
    int min_f=open_.begin()->second.f;
    noeud m_noeud=open_.begin()->second;
    for(it=open_.begin();it!=open_.end();++it){
        if(it->second.f<min_f){
            min_f=it->second.f;
            m_noeud=it->second;
        }
    }
    courant_=m_noeud;
}


void AStar::ajout_liste_ferme()
{

    open_.erase(courant_.p);
    closed_[courant_.p]=courant_;
}
void AStar::ajout_case_adjacentes()
{
    position base;
    noeud ajout;
    for(int i=0;i<4;i++){
        base=courant_.p;
        base.changePos(i);
        if( costmap_[base.y][base.x]<NX+NY && base.surLaMap() && !(dansLaListe(base,closed_)) ){

                ajout.p=base;
                ajout.g=courant_.g+costmap_[base.y][base.x];
                ajout.f=ajout.g+distance(base,fin_);
                ajout.parent=courant_.p;
                if(dansLaListe(base,open_)){
                    if(open_[base].f<ajout.f)
                        open_[base]=ajout;

                }else
                   open_[base]=ajout;

        }
    }
}

bool AStar::dansLaListe(position b,map<position,noeud> liste)
{
    map<position,noeud>::iterator i=liste.find(b);
    if(i!=liste.end())
        return true;
    else
        return false;

}
int AStar::distance(position a,position b)
{
    return abs(a.x-b.x)+abs(a.y-b.y);
}
position AStar::nextDir()
{
    if(chemin_){
        position p=fin_;
        while(closed_[p].parent!=depart_)
        {
            p=closed_[p].parent;
        }

        return p;
    }else
        return depart_;
}
vector<position> AStar::chemin()
{
    vector<position> retour;
    if(chemin_){
        position p=fin_;
        retour.push_back(p);
        while(closed_[p].parent!=depart_){
            p=closed_[p].parent;
            retour.push_back(p);
        }


    }else{
        retour.push_back(depart_);
    }

    return retour;

}
